import pynmea2

# Получение данных NMEA по UDP от программы-симулятора (nmea-gps-simulator)
import socket
import time
import random

NMEA_SIM_IP = "127.0.0.1"
NMEA_SIM_PORT = 10110

SERIAL_PORT = "/dev/ttyACM0"
SERIAL_BAUD_RATE = 115200

from pymavlink import mavutil

mavlink_gps_conn = mavutil.mavlink_connection('udpout:localhost:14540')
#mavlink_gps_conn = mavutil.mavlink_connection(SERIAL_PORT, baud=SERIAL_BAUD_RATE)

def mavlink_send_raw_gps(mav_conn, nmea_msg):
    # Выделение необходимой информации из NMEA сообщения
    lat = nmea_msg[0].latitude
    lon = nmea_msg[0].longitude
    alt = float(nmea_msg[0].altitude)
    # 0 = invalid, 1 = GPS fix, 2 = DGPS fix, etc.
    fix_quality = int(nmea_msg[0].gps_qual)  
    num_sats = int(nmea_msg[0].num_sats)
    hdop = float(nmea_msg[0].horizontal_dil) if hasattr(nmea_msg[0], 'horizontal_dil') else 65535

    # Преобразования
    lat_int = int(lat * 1e7)
    lon_int = int(lon * 1e7)
    alt_mm = int(alt * 1000)
    eph = int(hdop * 100) if hdop != 65535 else 65535
    epv = 65535
    vel = 0
    cog = 0
    satellites_visible = num_sats
    fix_type = 3 if fix_quality > 0 else 0

    time_usec = int(time.time() * 1e6)
    print(">>>\t", time_usec, "\t", lat_int, "\t", lon_int)
    
    # Отправка сообщения RAW_GPS_INT
    
    mav_conn.mav.gps_raw_int_send(
        time_usec,
        fix_type,
        lat_int + random.randint(0, 100),
        lon_int + random.randint(0, 100),
        alt_mm,
        eph,
        epv,
        vel,
        cog,
        satellites_visible
    )

with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
    sock.bind((NMEA_SIM_IP, NMEA_SIM_PORT))
    
    start_flag = False

    while True:
        seg, addr = sock.recvfrom(1024)

        try:
            msg_str = pynmea2.parse(seg.decode('utf-8'))
            #print(repr(msg_str))
        except pynmea2.ParseError as e:
            print('Parse error: {}'.format(e))
            continue
        
        if seg[1:6:1] == b'GPGGA':
            # Сброс сообщения
            raw_nmea_msg = seg
            full_msg = [msg_str]
            start_flag = True

        elif start_flag == True:
            raw_nmea_msg += seg
            full_msg.append(msg_str)
            
            #Отправка полного сообщения
            if seg[1:6:1] == b'GPZDA':
                #print(raw_nmea_msg.decode('utf-8'))
                #print(full_msg)
                freq = 20
                for i in range(0, freq, 1):
                    mavlink_send_raw_gps(mavlink_gps_conn, full_msg)
                    time.sleep(1 / freq)
